#include <bag_dealer/Projector.h>


namespace PercepViz
{
    void Projector::init(std::string cfg, std::string video)
    {
        auto pkg_path = boost::filesystem::path(ros::package::getPath("bag_dealer"));
        auto  cfg_path = pkg_path / "config" / cfg;
        ROS_WARN_STREAM(video);
        if (!boost::filesystem::exists(cfg_path))
            ROS_ERROR_STREAM("config file does not exist!");
        std::ifstream infile(cfg_path.string());
        double_t cameraIn[12];
        double_t RT[16];
        for (int i = 0; i < 12; i++){
            infile >> cameraIn[i];
        }
        cv::Mat(4, 4, 6, &cameraIn).copyTo(i_params.cameraIn);//cameraIn params

        for (int i = 0; i < 16; i++){
            infile >> RT[i];
        }
        cv::Mat(4, 4, 6, &RT).copyTo(i_params.RT);//lidar to camera params
        vWriter.open( (pkg_path / "video" / video).string(), CV_FOURCC('D', 'I', 'V', 'X'), 10,cv::Size(640,480));
    }

    Projector::Projector(std::string image_topic, std::string pc_topic, std::string cfg, std::string video)
    {
        ros::NodeHandle nh_("~");
        init(cfg, video);

        message_filters::Subscriber<sensor_msgs::Image> image_sub(nh_, image_topic, 10);
        message_filters::Subscriber<sensor_msgs::PointCloud2> pc_sub(nh_, pc_topic, 10);
        typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::PointCloud2> MySyncPolicy;
        message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), image_sub, pc_sub);
        sync.registerCallback(boost::bind(&Projector::callback, this, _1, _2));
        image_transport::ImageTransport imageTransport(nh_);
        image_pub = imageTransport.advertise("/project_pc_image", 20);

        ros::spin();
    }

    Projector::~Projector()
    {
        vWriter.release();
    }

    void Projector::callback(const sensor_msgs::ImageConstPtr& img,
                             const sensor_msgs::PointCloud2ConstPtr& pc)
    {
        cv_bridge::CvImagePtr cv_ptr;
        try {
            cv_ptr = cv_bridge::toCvCopy(img, "bgr8");
        }
        catch (cv_bridge::Exception &e) {
            return;
        }
        cv::Mat raw_img = cv_ptr->image;
        pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
        pcl::fromROSMsg(*pc, *cloud);
        cv::Mat visImg = raw_img.clone();
        cv::Mat overlay = visImg.clone();
        cv::Mat X(4,1,cv::DataType<double>::type);
        cv::Mat Y(3,1,cv::DataType<double>::type);

        for(pcl::PointCloud<pcl::PointXYZI>::const_iterator it = cloud->points.begin(); it != cloud->points.end(); it++){
            if(it->x > maxX || it->x < 0.0 || abs(it->y) > maxY || it->z < minZ )
            {
                continue;
            }

            X.at<double>(0,0) = it->x;
            X.at<double>(1,0) = it->y;
            X.at<double>(2,0) = it->z;
            X.at<double>(3,0) = 1;

            //Y = i_params.cameraIn * i_params.camtocam_mat * i_params.RT * X;  //tranform the point to the camera coordinate
            Y = i_params.cameraIn * i_params.RT * X;
            cv::Point pt;
            pt.x = Y.at<double>(0,0) / Y.at<double>(0,2);
            pt.y = Y.at<double>(1,0) / Y.at<double>(0,2);

            float val = it->x;
            float maxVal = 1.5;
            int red = std::min(255, (int)(255 * abs((val - maxVal) / maxVal)));
            int green = std::min(255, (int)(255 * (1 - abs((val - maxVal) / maxVal))));
            cv::circle(overlay, pt, 1, cv::Scalar(0, green, red), -1);
        }

        // Publish the image projection
        ros::Time time = ros::Time::now();
        cv_ptr->encoding = "bgr8";
        cv_ptr->header.stamp = time;
        cv_ptr->header.frame_id = "/camera_main";
        cv_ptr->image = overlay;
        vWriter << overlay;
        image_pub.publish(cv_ptr->toImageMsg());
    }
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "point_cloud_image_projector");
    if (argc < 4)
    {
        ROS_ERROR_STREAM("program arguments not set correctly! exit");
        exit(1);
    }
    PercepViz::Projector projector(argv[1], argv[2], argv[3], argv[4]);

    return 0;
}
